#ifndef MODEL_INVERSE_DYNAMICS_H
#define MODEL_INVERSE_DYNAMICS_H

#include "forward_kinematics.hpp"
#include "model/model_impl/basic_data_structure.hpp"

namespace ZORAL
{
    class InverseDynamics
    {
    private:
        ForwardKinematics *fk_;
        RobotModel *rm_;

    public:
        explicit InverseDynamics(ForwardKinematics *fk)
        {
            rm_ = fk->getRobotModel();
            fk_ = fk;
        }

        int inverseDynamics(const VectorXd &q, const VectorXd &qd, const VectorXd &qdd,
                            const std::vector<Wrench> &f_ext, VectorXd &torque);
    };
} // namespace ZORAL

#endif